#include "stm32f10x.h"                  // Device header
#include "MPU6050.h"
#include "MPU6050_Reg.h"
#include "MyI2C.h"
#include "Delay.h"
#include "math.h"

#define MPU6050_ADDR		0xD0		//MPU6050的I2C从机地址

// 传感器原始数据
typedef struct {
    int16_t accel[3];	// 数字量
    int16_t gyro[3];	// 数字量
} MPU6050_RawData;

// 传感器数据结构
typedef struct {
    float accel[3];		// m/s²
    float gyro[3];		// rad/s
} MPU6050_Data;

// 改进型互补滤波参数
//#define ALPHA_STATIC			0.98f
//#define ALPHA_DYNAMIC_MIN		0.85f
//#define ALPHA_DYNAMIC_MAX		0.98f
//#define ACCEL_TRUST_THRESH	0.5f

// 角度转换参数
#define M_PI				3.14159265358979323846f	// π
#define GRAVITY				9.80665f				// 重力
#define DEG_TO_RAD			0.017453292519943f  	// π/180, 角度转弧度
#define RAD_TO_DEG			57.2957795f  			// 180/π, 弧度转角度
#define CALIB_SAMPLES		500						// 校准采样点数

// 卡尔曼滤波器结构
typedef struct {
    float q_angle;   // 过程噪声
    float q_bias;    // 零偏噪声
    float r_measure; // 测量噪声
    float angle;     // 角度估计
    float bias;      // 零偏估计
    float p[2][2];   // 协方差矩阵
} Kalman;


/* ******************** 全局变量 ******************** */
static MPU6050_Data mpu;				// 陀螺仪数据
static MPU6050_Data mpu_bias;			// 陀螺仪零偏
static IMU_Attitude attitude = {0};		// 角度结构体初始化
static Kalman kalman_pitch = {0}, kalman_roll = {0};
static float dt = 0.01f;				// 默认5ms采样周期(200Hz)
static uint8_t is_calibrated = 0;		// 陀螺仪校准标志位


/**
  * 函    数：MPU6050初始化
  * 参    数：无
  * 返 回 值：无
  */
static void MPU6050_Init(void)
{
	MyI2C_Init();											// 先初始化底层的I2C

	/*MPU6050寄存器初始化，需要对照MPU6050手册的寄存器描述配置，此处仅配置了部分重要的寄存器*/
	I2C_WriteByte(MPU6050_ADDR, MPU6050_PWR_MGMT_1, 0x01);					// 电源管理寄存器1，取消睡眠模式，选择时钟源为X轴陀螺仪
	I2C_WriteByte(MPU6050_ADDR, MPU6050_PWR_MGMT_2, 0x00);					// 电源管理寄存器2，保持默认值0，所有轴均不待机
	I2C_WriteByte(MPU6050_ADDR, MPU6050_SMPLRT_DIV, 0x04);					// 采样率分频寄存器，配置采样率, 取值范围0到255, 1000/(取值+1)HZ
	I2C_WriteByte(MPU6050_ADDR, MPU6050_CONFIG, 0x01);						// 配置寄存器，配置DLPF:0~7
	I2C_WriteByte(MPU6050_ADDR, MPU6050_GYRO_CONFIG, GYRO_FS_SELECTED);		// 陀螺仪配置寄存器，选择满量程为±2000°/s  MPU6050_Reg.h里修改
	I2C_WriteByte(MPU6050_ADDR, MPU6050_ACCEL_CONFIG, ACCEL_FS_SELECTED);	// 加速度计配置寄存器，选择满量程为±16g

}

/**
  * 函    数：MPU6050获取数据
  * 参    数：mpu.accel 加速度计X、Y、Z轴的数据，使用输出参数的形式返回，范围：-32768~32767
  * 参    数：mpu.gyro 陀螺仪X、Y、Z轴的数据，使用输出参数的形式返回，范围：-32768~32767
  * 返 回 值：无
  */
static void MPU6050_GetData(void)
{
	uint8_t Buff[14];
	MPU6050_RawData rawmpu;
	
	I2C_ReadRaw(MPU6050_ADDR, MPU6050_ACCEL_XOUT_H, Buff, 14);
	
	// 解析数据，原始数据
	rawmpu.accel[0] = (int16_t)((Buff[0] << 8)  | Buff[1]);
	rawmpu.accel[1] = (int16_t)((Buff[2] << 8)  | Buff[3]);
	rawmpu.accel[2] = (int16_t)((Buff[4] << 8)  | Buff[5]);
	rawmpu.gyro[0]  = (int16_t)((Buff[8] << 8)  | Buff[9]);
	rawmpu.gyro[1]  = (int16_t)((Buff[10] << 8) | Buff[11]);
	rawmpu.gyro[2]  = (int16_t)((Buff[12] << 8) | Buff[13]);
	// 转化成对应单位
	mpu.accel[0] = rawmpu.accel[0]  * ACCEL_SCALE;		// ACCEL_SCALE在MPU6050_Reg.h定义了转化m/s²
	mpu.accel[1] = rawmpu.accel[1]  * ACCEL_SCALE;
	mpu.accel[2] = rawmpu.accel[2]  * ACCEL_SCALE;
	mpu.gyro[0]  = rawmpu.gyro[0]   * GYRO_SCALE;		// GYRO_SCALE在MPU6050_Reg.h定义了转化rad/s
	mpu.gyro[1]  = rawmpu.gyro[1]   * GYRO_SCALE;
	mpu.gyro[2]  = rawmpu.gyro[2]   * GYRO_SCALE;

}

/* ******************** 零偏校准 ******************** */
void MPU_Calib(void) 
{
	// 采集校准样本
	for(int i = 0; i < CALIB_SAMPLES; i++) 
	{
		MPU6050_GetData();
		
		for(int j = 0; j < 3; j++) 
		{
			mpu_bias.gyro[j]  += mpu.gyro[j];
			mpu_bias.accel[j] += mpu.accel[j];	  
		}
		
		Delay_ms(5);
	}
	// 计算平均零偏
	for(int i = 0; i < 3; i++) 
	{
		mpu_bias.gyro[i] /= CALIB_SAMPLES;
		mpu_bias.accel[i] /= CALIB_SAMPLES;
	}
	// 特殊处理：Z轴重力补偿
    // 静态时Z轴应为g，减去理论重力值
	mpu.accel[2] -= GRAVITY;
	is_calibrated = 1;
}

/* ******************** 卡尔曼滤波初始化 ******************** */
static void Kalman_Init(Kalman *k, float q_angle, float q_bias, float r_measure) 
{
	// 重置状态变量
    k->angle = 0.0f;
    k->bias = 0.0f;
	
	// 初始化协方差矩阵
    k->p[0][0] = 0.0f;
    k->p[0][1] = 0.0f;
    k->p[1][0] = 0.0f;
    k->p[1][1] = 0.0f;
	
	// 设置噪声参数 (需根据实际传感器调整)
    k->q_angle = q_angle;		// 0.001f, 角度过程噪声, 调整范围：0.0001~0.01(增大=更信任陀螺仪)
    k->q_bias = q_bias;			// 0.003f, 零偏过程噪声, 调整范围：0.001~0.01(增大=更快跟踪零偏变化)
    k->r_measure = r_measure;	// 0.03f, 测量噪声, 调整范围：0.01~0.1(增大=更信任加速度计)
}

/* ******************** 卡尔曼滤波更新 ******************** */
static float Kalman_Update(Kalman *k, float new_angle, float new_rate) 
{
	// 预测步骤
	k->angle   += dt * (new_rate - k->bias);
	k->p[0][0] += dt * (dt * k->p[1][1] - k->p[0][1] - k->p[1][0] + k->q_angle);
	k->p[0][1] -= dt * k->p[1][1];
	k->p[1][0] -= dt * k->p[1][1];
	k->p[1][1] += k->q_bias * dt;
	
	// 更新步骤
	float s = k->p[0][0] + k->r_measure;
	float k0 = k->p[0][0] / s;
	float k1 = k->p[1][0] / s;
	
	float y = new_angle - k->angle;
	k->angle += k0 * y;
	k->bias  += k1 * y;
	
	k->p[0][0] -= k0 * k->p[0][0];
	k->p[0][1] -= k0 * k->p[0][1];
	k->p[1][0] -= k1 * k->p[0][0];
	k->p[1][1] -= k1 * k->p[0][1];
	
	return k->angle;
}

/* ******************** 姿态解算核心 ******************** */
static void Attitude_Update(void) 
{
    // 1. 读取并预处理传感器数据
    MPU6050_GetData();
	
	// 应用校准
    if(is_calibrated) 
	{
        for(int i = 0; i < 3; i++) 
		{
            mpu.gyro[i]  -= mpu_bias.gyro[i];
			mpu.accel[i] -= mpu_bias.accel[i];
        }
    }
	
    // 2. 从加速度计计算角度
    float acc_pitch = atan2f(-mpu.accel[0], 
                           sqrtf(mpu.accel[1] * mpu.accel[1] + 
                                 mpu.accel[2] * mpu.accel[2]));
    
    float acc_roll  = atan2f(mpu.accel[1], mpu.accel[2]);
    
    // 3. 卡尔曼后滤波
    attitude.pitch = Kalman_Update(&kalman_pitch, acc_pitch, mpu.gyro[1]);
    attitude.roll  = Kalman_Update(&kalman_roll, acc_roll, mpu.gyro[0]);
    
    // 4. 转换为角度值
    attitude.pitch_deg = attitude.pitch * RAD_TO_DEG;
    attitude.roll_deg  = attitude.roll * RAD_TO_DEG;
}

/* ******************** 公共接口实现 ******************** */
/**
  * 函    数：MPU6050获取ID号
  * 参    数：无
  * 返 回 值：MPU6050的ID号
  */
uint8_t MPU6050_GetID(void)
{
	return I2C_ReadByte(MPU6050_ADDR, MPU6050_WHO_AM_I);		// 返回WHO_AM_I寄存器的值
}

void IMU_Init(void)
{
    // 初始化MPU6050
    MPU6050_Init();
    
    // 初始化卡尔曼滤波器
    Kalman_Init(&kalman_pitch, 0.001f, 0.003f, 0.03f);
    Kalman_Init(&kalman_roll, 0.001f, 0.003f, 0.03f);
}

void IMU_Update(void) 
{
    Attitude_Update();
}

IMU_Attitude IMU_GetAttitude(void) 
{
    return attitude;
}
